#ifndef GEOM_EQUATIONS_H_
#define GEOM_EQUATIONS_H_

#include "Vec3.h"

namespace physicsworks {

namespace math {

/**
 * Represents a plane equation.
 * The equation is normal * X = distance, where
 * X is any point on the plane.
 */
template<class T>
class Plane {
private:
	Vec3<T> normal; /**< The plane's normal. */
	T distance; /**< Distance from origin. */
	
public:
	/**
	 * Constructs the plane from three non-linear points.
	 */
	Plane(const Vec3<T>& v0, const Vec3<T>& v1, const Vec3<T>& v2) :
	 normal( (v1 - v0).cross(v2 - v0).normalize() ), distance( normal * v0 ) {}
	
	const Vec3<T>& getNormal() const { return normal; }
	T getDistance() const { return distance; }
	bool inFront(const Vec3<T>& v) const { return v * normal > distance;          }
	bool inBack (const Vec3<T>& v) const { return v * normal < distance;          }
	bool inside (const Vec3<T>& v) const { return equal<T>(v * normal, distance); }
};

/**
 * Represents a line equation.
 * This equation is X = point + t*direction, where
 * X is any point on the line.
 */
template<class T>
class Line {
private:
	Vec3<T> direction; /**< Direction of the line. */
	Vec3<T> point; /**< A point on the line. */

public:
	/**
	 * Constructs the line from two different points.
	 * Direction will be the vector D = v1 - v0 normalized.
	 */
	Line(const Vec3<T>& v0, const Vec3<T>& v1) : direction(v1 - v0), point(v0) {
		direction.normalize();
	}

	/**
	 * Calculate the line equation generated from the intersection of two planes.
	 */
	Line(const Plane<T>& p0, const Plane<T>& p1) : direction( p0.getNormal().cross(p1.getNormal()) ), point() {
		
		direction.normalize();
		
		Mat<real> m(2, 3);
		
		m(0, 2) = p0.getDistance();
		m(1, 2) = p1.getDistance();
		
		Vec<real> sol(2);
		
		if (std::abs(direction.y) > std::abs(direction.x)) {
			if (std::abs(direction.z) > std::abs(direction.y)) {
				
				m(0, 0) = p0.getNormal().x; m(0, 1) = p0.getNormal().y;
				m(1, 0) = p1.getNormal().x; m(1, 1) = p1.getNormal().y;
				Assert<EBadArg>(m.solve(sol), "This system must has exactly one solution");
				point(sol(0), sol(1), 0);
				
			} else {
				
				m(0, 0) = p0.getNormal().x; m(0, 1) = p0.getNormal().z;
				m(1, 0) = p1.getNormal().x; m(1, 1) = p1.getNormal().z;
				Assert<EBadArg>(m.solve(sol), "This system must has exactly one solution");
				point(sol(0), 0, sol(1));
				
			}
		} else {
			if (std::abs(direction.x) > std::abs(direction.z)) {
				
				m(0, 0) = p0.getNormal().y; m(0, 1) = p0.getNormal().z;
				m(1, 0) = p1.getNormal().y; m(1, 1) = p1.getNormal().z;
				Assert<EBadArg>(m.solve(sol), "This system must has exactly one solution");
				point(0, sol(0), sol(1));
				
			} else {
				
				m(0, 0) = p0.getNormal().x; m(0, 1) = p0.getNormal().y;
				m(1, 0) = p1.getNormal().x; m(1, 1) = p1.getNormal().y;
				Assert<EBadArg>(m.solve(sol), "This system must has exactly one solution");
				point(sol(0), sol(1), 0);
				
			}
		}
		
	}
	
	const Vec3<T>& getDirection() const { return direction; }
	const Vec3<T>& getPoint() const { return point; }
};

template<class T> inline T calcTriangleArea(const Vec3<T>& v0, const Vec3<T>& v1, const Vec3<T>& v2)
{
	// TODO: implement it
}

/**
 * Calculates the triangle area in 2D space.
 * \TODO verify implementation, robustness and execution speed.
 */
template<class T> inline T calcTriangleArea(const Vec2<T>& v0, const Vec2<T>& v1, const Vec2<T>& v2)
{
	const Vec2<T> B = v0 - v1;
	const T b = B.length();
	
	return b * ( std::abs(B.rotate90CounterCC().normalize() * (v2 - v1)) ) * 0.5;
}

}

}

#endif /*GEOM_EQUATIONS_H_*/
